Potential Fields in 3 Joint Space
By Brian Lesko, Morgan Strauss
Graduate Researcher and Teaching Associate, Masters of mechanical engineering student
For Robotics Project | Robotics 7752 | 11/29/2022
Introduction
This live script characterizes the development journey through a path planning algorithm reffered to as the potential fields method - authored by Brian Lesko in collaboration with Morgan Strauss.
The potential field method is applied here in the 3 joint space for an RRP robot having its revolute joints overlap at the origin. This architecture exists in the robot produced by Intuitive Surgical known as the dVRK. It is used by surgeons to teleoperate, often, multiple of these robots are used all with end effector control separate from the positional control demonstrated in this script.
Motivation
There are multiple different existing path planning algorithms that exist which could potentially be used to solve this problem. One of these algorithms is PRM, in which a roadmap of the free space of the robot is created and an efficient path through this space is solved using a searching algorithm such as A*. The upside to this option is that once the roadmap is created an optimal path can be quickly built around any obstacles. The drawbacks are that a representation of the available free space must be created, and there are multiple options as to how to sample the joint space and detect collision points. Another option that was considered was to use RRT*, or a variation of the rapidly exploring random trees method which continuously improves its initial solution to find a time or distance efficient path. This method will improve the solution over time and approach an optimally short path, but it takes multiple attempted paths to reach a solution which can be more computationally intensive than our chosen method.
The path planning algorithm that was chosen for this project was the virtual potential fields method. This method was chosen due to it being more intuitive and for the ability to check the accuracy of the model through plots of the potentials and gradients that were created. The main downside to the virtual potential fields’ method is that you can get trapped in local minima. To prevent this, the strength of the potential fields and gradients for the goal and the obstacles can be altered to prevent the algorithm from falling into these local minimums
Start of Script
AKA Stanford Robot Architecture
[Slist,Mlist_joints] = getdVRKArchitecture();
A Robot Configuration, described
The robot configuration in the task space can be described as a list of occupied points, these points will then be used later to create an obstacle potential field
% Forward Kinematics to find all joint locations
jointLocations = getJointLocations(Slist(:,1:3),Mlist_joints,theta);
% add n points lying between each joint on their respective link
n = 3; % Test Points per Link
configurationPoints = getConfigurationPoints(jointLocations,n);
% the 1st two joints of the dVRK are overlapping, throw out points from the "link" between them
configurationPoints = configurationPoints(3:length(configurationPoints(:,1)),:);
Get potential of configuration with respect to an obstacle
The obstacle potential over the joint space can be found using a sum of distances between points along the prismatic link on the robot and an obstacle surface
obstacle = [.04 .04 -.1 .02];
potential = getObstaclePotentialAtConfigurationPoints(configurationPoints,obstacle)
The obstacle and the points along the primsatic link of the robot on the joint space can then be plotted.
plotMesh(configurationPoints); hold on
sphereMesh = getSphereMesh(obstacle(1:3)',obstacle(4),10);
plotMesh(sphereMesh); hold off
Repeat for all Obstacles, sum
The obstacle potential for each obstacle can then summed to get an overall obstacle potential.
obstacles = [[1 1 1 0];[2 2 2 0]];
potentials = zeros(length(obstacles(:,1)),1);
for i = 1:1:length(obstacles(:,1))
potentials(i) = getObstaclePotentialAtConfigurationPoints(configurationPoints,obstacles(i,:));
potential = sum(potentials)
Unobstructed Configuration Space of the Robot
To describe the entire configuration space of the robot, a number of gridpoints is set for each angle. The joint space is then described by gridded points, specific for RRP, with joint limits as the end points of the grid.
jointMaxes = [[50 50]*pi/180 .15];
jointMins = [[-50 -50]*pi/180 .028];
thetas = getCSpacePermutations(nGridPoints,jointMaxes,jointMins);
Define a Goal Position in the joint space
A goal position in the joint space and task space is then created by calculating the squared distance from every configuration to the goal position.
M3 = Mlist_joints(:,:,1)*Mlist_joints(:,:,2)*Mlist_joints(:,:,3);
goalTheta = [[40 40]*pi/180 .12]';
Goal Potential Field
The goal position in the joint space is used to find the goal potential over the entire joint space, which is then plotted.
goalPotentialField = getGoalPotentialField(goalTheta,nGridPoints); plotStartEnd(startTheta,goalTheta)
A Highly Gridded Potential Field
Goal Gradient Field
Using the equation from the textbook, the goal gradient is then calculated and then plotted.
goalGradientField = getGoalGradient(goalTheta,goalPotentialField); plotStartEnd(startTheta,goalTheta)
A Gradient Field with a low number of gridpoints
The Obstacle Potential Field
This uses the same methods as before to calculate an overall obstacle potential, and plot it.
obstacles = [1.837 -0.399 -126.255 20]/1000; %a ball near the origin
obstaclesTheta = [0 0.02 0.1328] % its position in theta space
obstaclePotentialField = getObstaclePotentialField(Slist(:,1:3),Mlist_joints,thetaMesh,obstacles);
obP = obstaclePotentialField;
scatter3(obP(:,1),obP(:,2),obP(:,3),[],obP(:,4),'o'),colorbar
title('Obstacle Potential Field')
Obstacle Gradient Field
Using the textbook equations, the obstacle gradient can also be calculated for every configuration.
obstacleGradientField = getObstacleGradientField(Slist(:,1:3),Mlist_joints,thetaMesh,obstacles,obstaclesTheta); plotStartEnd(startTheta,goalTheta)
Combining the Fields
This section is used to change the weights of each potential field and add them together
gradientField = obstacleGradientField;
gradientField(:,i) = totalScale*(.05*obstacleGradientField(:,i) + 3*goalGradientField(:,i));
quiver3(gf(:,1),gf(:,2),gf(:,3),gf(:,5),gf(:,6),gf(:,7),'b')
view([-29.671 -33.270]), title('Total Potential Field')
plotStartEnd(startTheta,goalTheta)
xlabel('\theta_1'), ylabel('\theta_2'), zlabel('\theta_3')
Creating a Path through the Gradient Field
Using the control law and the calculated gradients, a trajectory to the goal position can be found:
trajectory = getPathThroughGradientField(startTheta,gradientField,goalTheta,dt)
lastTheta = trajectory(length(trajectory(:,1)),:)
T = FKinSpace(M3, Slist(:,1:3), lastTheta'); goalPositionFromTrajectory = T(1:3,4)'
Trajectory Through the Field
scatter3(P(:,1),P(:,2),P(:,3),[],P(:,4),'o'),colorbar
% quiver3(gf(:,1),gf(:,2),gf(:,3),gf(:,5),gf(:,6),gf(:,7),'b')
plot3(startTheta(1),startTheta(2),startTheta(3),'ko')
plot3(goalTheta(1),goalTheta(2),goalTheta(3),'ro')
plot3(trajectory(:,1),trajectory(:,2),trajectory(:,3),'--m')
The Goal Position is reached by this joint space trajectory
goalPositionFromTrajectory
Animating the Produced Motion
filename = 'gifBABY.gif';
pointMesh = getSphereMesh(obstacles(1,1:3)',obstacles(1,4),10);
animateRobot4(trajectory,timeTotal,Slist,Mlist_joints,filename,pointMesh)
Potential Fields Animation